/************************************************************************
* math_quaternion.c
* voxelands - 3d voxel world sandbox game
* Copyright (C) Lisa 'darkrose' Milne 2016 <lisa@ltmnet.com>
*
* This program is free software: you can redistribute it and/or modify
* it under the terms of the GNU General Public License as published by
* the Free Software Foundation, either version 3 of the License, or
* (at your option) any later version.
*
* This program is distributed in the hope that it will be useful, but
* WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.
* See the GNU General Public License for more details.
*
* You should have received a copy of the GNU General Public License
* along with this program.  If not, see <http://www.gnu.org/licenses/>
************************************************************************/

#include "common.h"

#include <math.h>
#include <stdlib.h>
#include <stdio.h>

/* create a new quaternion */
quaternion_t *quat_create_quat(float x, float y, float z, float w)
{
	quaternion_t *q = malloc(sizeof(quaternion_t));

	quat_init_quat(q,x,y,z,w);

	return q;
}

/* create a new quaternion from euler values */
quaternion_t *quat_create_euler(float x, float y, float z)
{
	quaternion_t *q = malloc(sizeof(quaternion_t));

	quat_init_euler(q,x,y,z);

	return q;

}

/* create a new quaternion from axis values */
quaternion_t *quat_create_axis(v3_t *v, float angle)
{
	quaternion_t *q = malloc(sizeof(quaternion_t));

	quat_init_axis(q,v,angle);

	return q;

}

/* init a quaternion */
void quat_init_quat(quaternion_t *q, float x, float y, float z, float w)
{
	q->x = x;
	q->y = y;
	q->z = z;

	q->w = w;

	quat_normalise(q);
}

/* init a quaternion from euler values */
void quat_init_euler(quaternion_t *q, float x, float y, float z)
{
	float poa = M_PI/180.0;
	float rx = y * poa / 2.0;
	float ry = z * poa / 2.0;
	float rz = x * poa / 2.0;

	float sinp = sin(rx);
	float siny = sin(ry);
	float sinr = sin(rz);
	float cosp = cos(rx);
	float cosy = cos(ry);
	float cosr = cos(rz);

	q->x = sinr * cosp * cosy - cosr * sinp * siny;
	q->y = cosr * sinp * cosy + sinr * cosp * siny;
	q->z = cosr * cosp * siny - sinr * sinp * cosy;
	q->w = cosr * cosp * cosy + sinr * sinp * siny;

	quat_normalise(q);
}

/* init a quaternion from axis values */
void quat_init_axis(quaternion_t *q, v3_t *v, float angle)
{
	float rad;
	float scale;

	if (sqrt((v->x*v->x)+(v->y*v->y)+(v->z*v->z)) == 0.0) {
		q->w = 1.0f;
		q->x = 0.0f;
		q->y = 0.0f;
		q->z = 0.0f;

		return;
	}

	rad = angle / 2.0;
	q->w = cosf(rad);

	scale = sinf(rad);

	q->x = (v->x*scale);
	q->y = (v->y*scale);
	q->z = (v->z*scale);

	quat_normalise(q);
}

/* normalise a quaternion */
void quat_normalise(quaternion_t *q)
{
	float mag = sqrt((q->x*q->x)+(q->y*q->y)+(q->z*q->z)+(q->w*q->w));

	/* Check for bogus length, to protect against divide by zero */
	if (mag != 0.0) {
		q->w /= mag;
		q->x /= mag;
		q->y /= mag;
		q->z /= mag;
	}
}

/* get axis values from a quaternion */
void quat_get_axis(quaternion_t *q, v3_t *v, float *angle)
{
	double scale;

	*angle = acos(q->w)*2.0;

	scale = sqrt((q->x*q->x)+(q->y*q->y)+(q->z*q->z));

	if (!scale) {
		*angle = 0.0f;

		v->x = 0.0f;
		v->y = 0.0f;
		v->z = 1.0f;
	}else{
		v->x = q->x/scale;
		v->y = q->y/scale;
		v->z = q->z/scale;
	}

	vect_normalise(v);
}

/* multiply a quaternion by a vector */
void quat_multiply_vector(quaternion_t *q, quaternion_t *rq, v3_t *v)
{
	rq->w = -((q->x*v->x)-(q->y*v->y)-(q->z*v->z));
	rq->x = (q->w*v->x)+(q->y*v->z)-(q->z*v->y);
	rq->y = (q->w*v->y)+(q->z*v->x)-(q->x*v->z);
	rq->z = (q->w*v->z)+(q->x*v->y)-(q->y*v->x);
}

/* multiply a quaternion by a quaternion */
void quat_multiply(quaternion_t *q1, quaternion_t *q2, quaternion_t *rq)
{
	rq->w = (q1->w*q2->w)-(q1->x*q2->x)-(q1->y*q2->y)-(q1->z*q2->z);
	rq->x = (q1->w*q2->x)+(q1->x*q2->w)+(q1->y*q2->z)-(q1->z*q2->y);
	rq->y = (q1->w*q2->y)+(q1->y*q2->w)+(q1->z*q2->x)-(q1->x*q2->z);
	rq->z = (q1->w*q2->z)+(q1->z*q2->w)+(q1->x*q2->y)-(q1->y*q2->x);
}

/* print the values of a quaternion */
void quat_print(quaternion_t *q)
{
	printf("w:%f, x:%f, y:%f, z:%f",q->w,q->x,q->y,q->z);
}


/* compute the w value of a quaternion */
void quat_computew(quaternion_t *q)
{
	float t = 1.0-(q->x*q->x)-(q->y*q->y)-(q->z*q->z);

	if (t < 0.0) {
		q->w = 0.0;
	}else{
		q->w = -sqrt(t);
	}
}

/* get the world position of a vertex, from a joint/weight */
void quat_rotate(quaternion_t *q, v3_t *in, v3_t *out)
{
	quaternion_t tmp;
	quaternion_t inv;
	quaternion_t qin;

	inv.x = -q->x;
	inv.y = -q->y;
	inv.z = -q->z;
	inv.w =  q->w;

	qin.x = in->x;
	qin.y = in->y;
	qin.z = in->z;
	qin.w = 0;

	quat_multiply(q, &qin, &tmp);
	quat_multiply(&tmp, &inv, &qin);

	out->x = qin.x;
	out->y = qin.y;
	out->z = qin.z;
}
